#include "rvo_node.h"

uint64_t seq = 0;       // 定义全局变量 seq 用于记录消息序列号
std::string agent_name; // 定义全局变量 agent_name 用于存储智能体名称
double iris_initial_pos_x;
double iris_initial_pos_y;
double iris_initial_pos_z;
int num_publishers = 1;

int main(int argc, char **argv)
{
    // 初始化ROS节点
    std::string iris_id = argv[1];
    ros::init(argc, argv, "rvo_node_" + iris_id);
    ros::NodeHandle n;

    // 广播rvo_vel话题
    // rvo_node_pub = n.advertise<gazebo_msgs::WorldState>("rvo_vel", 1000);

    // 订阅/gazebo/model_states话题
    // ros::Subscriber sub = n.subscribe("/gazebo/model_states", 100, rvo_velCallback);

    ros::Subscriber sub = n.subscribe("/iris_" + iris_id + "/uav_pos_vel", 100, rvo_true_velCallback);

    // 广告set_rvo_goals服务
    std::string service_name = "set_rvo_goals_" + iris_id;
    ros::ServiceServer service = n.advertiseService(service_name, set_goals);

    // 发布到xtdrone

    // 初始化每个发布者并添加到向量中

    std::string topic_name = "/xtdrone/iris_" + iris_id + "/cmd_vel_flu";
    ros::Publisher pub = n.advertise<geometry_msgs::Twist>(topic_name, 1000);
    // single_cmd_vel_flu_pub.push_back(pub);
    single_cmd_vel_flu_pub_new = pub;

    // 设置循环频率为50Hz
    ros::Rate loop_rate(50);

    // 处理命令行参数
    if ((argc > 1) && (argc % 3 == 1))
    {
        int num_init_point = argc - 1;
        for (int i = 1; i < num_init_point + 1; i = i + 3)
        {
            geometry_msgs::Point point;
            point.x = atof(argv[i]);
            point.y = atof(argv[i + 1]);
            point.z = atof(argv[i + 2]);
            rvo_goals.push_back(point);
        }
    }
    else
    {
        ROS_INFO("No input, Using default position 0 1 0 2 ....0 10 ");
    }

    // 获取参数服务器中的参数
    double neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, goal_threshold;
    n.param<double>("neighborDist", neighborDist, 4);
    n.param<double>("maxNeighbors", maxNeighbors, 10);
    n.param<double>("timeHorizon", timeHorizon, 10);
    n.param<double>("timeHorizonObst", timeHorizonObst, 5);
    n.param<double>("radius", radius, 0.3);
    n.param<double>("maxSpeed", maxSpeed, 0.2);
    n.param<double>("goal_threshold", goal_threshold, 0.01);
    n.param<std::string>("agent_name", agent_name, "iris_");
    n.param<double>("iris_initial_pos_x", iris_initial_pos_x, 0);
    n.param<double>("iris_initial_pos_y", iris_initial_pos_y, 0);
    n.param<double>("iris_initial_pos_z", iris_initial_pos_z, 0);

    // 创建RVOPlanner对象并进行初始化
    rvo = new RVO::RVOPlanner("gazebo");

    rvo->goal_threshold = goal_threshold;
    rvo->setupScenario(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed);
    rvo_goals_init();

    std::cout << "Configure completely" << std::endl;

    // 主循环，持续运行节点
    while (ros::ok())
    {
        ros::spinOnce();   // 处理所有挂起的回调函数
        loop_rate.sleep(); // 休眠以使循环以指定频率运行
    }
}

// 用于处理 SetGoals 服务请求。该函数根据请求中指定的模型类型（"default", "circle", "circle_spin", "random"）设置目标点，并返回目标点的数量。
bool set_goals(rvo_ros::SetGoals::Request &req, rvo_ros::SetGoals::Response &res)
{
    // 处理 "default" 模型类型
    if (req.model == "default")
    {
        // 设置当前运动模型为请求中的模型类型
        motion_model = req.model;

        // 清空现有的目标点列表
        if (!rvo_goals.empty())
            rvo_goals.clear();

        // 将请求中的坐标点添加到目标点列表中
        for (const auto &coordinate : req.coordinates)
        {
            rvo_goals.push_back(coordinate);
        }

        // 设置响应中的目标点数量
        res.num_goal = rvo_goals.size();

        return true;
    }


    // 如果模型类型不匹配，返回错误信息
    std::cout << "The specific model is wrong" << std::endl;
    return false;
}

// 用于初始化 rvo_goals 列表。如果 rvo_goals 列表为空，该函数会将一系列预定义的目标点添加到列表中。
// 用于在系统启动或重置时初始化目标点列表，确保 rvo_goals 列表中有一组预定义的目标点。
void rvo_goals_init()
{
    // 检查目标点列表是否为空
    if (rvo_goals.empty())
    {
        // 如果目标点列表为空，初始化目标点列表
        for (int i = 0; i < num_max; i++)
        {
            // 创建一个新的几何消息点
            geometry_msgs::Point point;

            // 设置点的坐标
            if (i == 0)
            {
                point.x = iris_initial_pos_x;
                point.y = iris_initial_pos_y;
                point.z = iris_initial_pos_z;

                // std::cout << "iris_initial_pos_x: " << iris_initial_pos_x << " iris_initial_pos_y: " << iris_initial_pos_y << std::endl;
            }
            else
            {
                point.x = 2 * i; // x 坐标设置为索引值的浮点数表示
                point.y = 0;     // y 坐标设置为 1.0
                point.z = 2.0;   // z 坐标设置为 0.0
            }
            // 将点添加到目标点列表中
            rvo_goals.push_back(point);
        }
    }
}



void rvo_true_velCallback(const UAVPosVel::ConstPtr &sub_msg)
{
    // 增加序列号
    seq++;
    int count_vel = 0;

    // 更新智能体的状态
    // rvo->updateState_gazebo(sub_msg, agent_name);
    rvo->updateState_true(sub_msg);

    // 根据不同的运动模型设置目标
    if (motion_model == "default")
        rvo->setGoal(rvo_goals);

    // 设置智能体的初始状态和偏好速度
    rvo->setInitial();
    rvo->setPreferredVelocities();

    // 检查是否所有智能体都到达目标
    bool arrive_flag = rvo->arrived();

    // 计算新的速度
    std::vector<RVO::Vector3 *> new_velocities = rvo->step();

    // 设置消息头
    std_msgs::Header header;
    header.seq = seq;
    header.stamp = ros::Time::now();
    header.frame_id = "/world";

    msg_pub.header = header;
    msg_pub.name.clear();
    msg_pub.pose.clear();
    msg_pub.twist.clear();

    // my_msg.twist.clear();

    num_agent = new_velocities.size();
    // std::cout<<num_agent;

    // ROS_INFO(""+copy_num_agent);

    // 如果智能体数量发生变化，输出智能体数量
    if (num_agent != copy_num_agent)
    {
        std::cout << "The num of agents is " + std::to_string(num_agent) << std::endl;
        copy_num_agent = num_agent;
    }

    // 遍历发布更新后的速度信息
    geometry_msgs::Twist new_vel;

    // 如果找到了对应的智能体名称，更新速度
    // 用哪架无人机，序号改成几
    float x = new_velocities[0]->x();
    float y = new_velocities[0]->y();
    float z = new_velocities[0]->z();

    new_vel.linear.x = x;
    new_vel.linear.y = y;
    new_vel.linear.z = z;

    // my_msg.twist.push_back(new_vel);
    // single_cmd_vel_flu_pub[0].publish(new_vel);
    single_cmd_vel_flu_pub_new.publish(new_vel);
}